#ifndef _CHASSIS_H_
#define _CHASSIS_H_

#include "main.h"
#include "xc330_motor.h"
#include "protocol.h"

typedef struct
{
    float target_vbx_, target_vby_, target_wbz_; // target position and orientation of the end-effector in body frame
} cmd_recv_s;

void chassis_init(void);
void chassis_task(void);
void chassis_cmd_recv_decode(void);
void chassis_cmd_send_feedback(void);

void forward_kinematics(float uf, float ur, float ul);
void inverse_kinematics(float vx, float vy, float wz);
#endif